using System;
using System.Diagnostics;
using Latios.Transforms;
using Unity.Collections;
using Unity.Mathematics;

namespace Latios.Psyshock
{
    public static partial class UnitySim
    {
        /// <summary>   A contact jacobian angular. </summary>
        public struct ContactJacobianAngular
        {
            /// <summary>   The angular a. </summary>
            public float3 angularA;
            /// <summary>   The angular b. </summary>
            public float3 angularB;
            /// <summary>   The effective mass. </summary>
            public float effectiveMass;
        }

        /// <summary>   A contact jacobian angle and velocity for a contact point to reach the contact plane. </summary>
        public struct ContactJacobianContactParameters
        {
            /// <summary>   The jacobian. </summary>
            public ContactJacobianAngular jacobianAngular;

            /// <summary>
            /// Velocity needed to reach the contact plane in one frame, both if approaching (negative) and
            /// depenetrating (positive)
            /// </summary>
            public float velocityToReachContactPlane;

            /// <summary>
            /// The distance to the contact point. This point is initially identified by the narrowphase and is initialized
            /// during the Jacobian Build stage. During the solve and integrate stage, this value is updated each substep.
            /// </summary>
            public float contactDistance;

            /// <summary>
            /// A flag used to indicate if an impulse should be applied to the contact point. This flag is set to true by
            /// default and is only false if: the substepCount > 1, the coefficient of restitution is greater than 0 and
            /// CalculateRestitution returns false. Needed for brief contacts when substepping so that impulse isn't always
            /// applied during Solve.
            /// </summary>
            public bool applyImpulse;
        }

        /// <summary>
        /// Contact body friction and surface normal values.
        /// </summary>
        public struct ContactJacobianBodyParameters
        {
            // Todo: Better understand these and document them.

            // Linear friction
            public ContactJacobianAngular friction0;  // effectiveMass stores friction effective mass matrix element (0, 0)
            public ContactJacobianAngular friction1;  // effectiveMass stores friction effective mass matrix element (1, 1)
            public float3                 frictionDirection0;
            public float3                 frictionDirection1;

            // Angular friction about the contact normal, no linear part
            public ContactJacobianAngular angularFriction;  // effectiveMass stores friction effective mass matrix element (2, 2)
            public float3                 frictionEffectiveMassOffDiag;  // Effective mass matrix (0, 1), (0, 2), (1, 2) == (1, 0), (2, 0), (2, 1)

            public float3 centerA;
            public float3 centerB;

            public float3 contactNormal;
            public float3 surfaceVelocityDv;

            public float coefficientOfFriction;
            public float coefficientOfRestitution;

            public float substep;

            public void SetSurfaceVelocity(Velocity surfaceVelocity)
            {
                surfaceVelocityDv = default;
                if (!surfaceVelocity.Equals(float3.zero))
                {
                    float linVel0 = math.dot(surfaceVelocity.linear, frictionDirection0);
                    float linVel1 = math.dot(surfaceVelocity.linear, frictionDirection1);

                    float angVelProj  = math.dot(surfaceVelocity.angular, contactNormal);
                    surfaceVelocityDv = new float3(linVel0, linVel1, angVelProj);
                }
            }
        }

        /// <summary>
        /// Impulses generated by the contact during a SolveJacobian call
        /// </summary>
        public struct ContactJacobianImpulses
        {
            /// <summary>
            /// The magnitude of the impulse applied by all contact points
            /// </summary>
            public float combinedContactPointsImpulse;
            /// <summary>
            /// The impulse from friction applied in the primary friction axis
            /// </summary>
            public float friction0Impulse;
            /// <summary>
            /// The impulse from friction applied in the secondary friction axis
            /// </summary>
            public float friction1Impulse;
            /// <summary>
            /// The impulse from friction applied via rotational friction about the contact
            /// </summary>
            public float frictionAngularImpulse;
        }

        /// <summary>
        /// A filter that can be applied to contacts during solving to eliminate ghost collisions.
        /// When the A collider is static, contact points on B are raycasted against colliderA
        /// from the initial contact point to the speculated ending location of the contact point
        /// based on the current velocity. If the ray misses, the contact is ignored for the solve
        /// iteration. The roles flip if the B collider is static. The contact speculation is copied
        /// from Unity Physics as-is, bugs and all.
        /// </summary>
        public struct ContactJacobianStaticCollisionFilter
        {
            internal Collider      collider;
            internal TransformQvvs transform;
            internal float3        contactPointOnB;
            internal float3        centerOfMass;
            internal float3        contactDistanceDirectionFactor;
            internal int           subCollider;
            internal bool          aIsStatic;
            internal bool          enabled;

            public ContactJacobianStaticCollisionFilter(in Collider colliderA, in TransformQvvs transformA, in RigidTransform inertialPoseWorldTransformA,
                                                        in Collider colliderB, in TransformQvvs transformB, in RigidTransform inertialPoseWorldTransformB,
                                                        in ColliderDistanceResult distanceResult, in ContactsBetweenResult contactsResult, bool aIsStatic)
            {
                if (aIsStatic)
                {
                    this.collider                       = colliderA;
                    this.transform                      = transformA;
                    this.contactPointOnB                = contactsResult[contactsResult.contactCount - 1].location;
                    this.centerOfMass                   = inertialPoseWorldTransformA.pos;
                    this.contactDistanceDirectionFactor = float3.zero;
                    this.subCollider                    = distanceResult.subColliderIndexA;
                    this.aIsStatic                      = aIsStatic;
                    this.enabled                        = true;
                }
                else
                {
                    this.collider                       = colliderB;
                    this.transform                      = transformB;
                    this.contactPointOnB                = contactsResult[contactsResult.contactCount - 1].location;
                    this.centerOfMass                   = inertialPoseWorldTransformB.pos;
                    this.contactDistanceDirectionFactor = contactsResult.contactNormal;
                    this.subCollider                    = distanceResult.subColliderIndexB;
                    this.aIsStatic                      = aIsStatic;
                    this.enabled                        = true;
                }
            }
        }

        /// <summary>
        /// Unity's default maximum depenetration velocity between a dynamic and static object
        /// </summary>
        public const float kMaxDepenetrationVelocityDynamicStatic = float.MaxValue;
        /// <summary>
        /// Unity's default maximum depenetration velocity between two dynamic objects
        /// </summary>
        public const float kMaxDepenetrationVelocityDynamicDynamic = 3f;

        // Notes: In Unity Physics, inertialPoseWorldTransformA/B is identity if the body is static.
        // perContactParameters can be uninitialized.
        // gravityAgainstContactNormal uses the magnitude of global gravity. Would a dot product with the contact normal be more appropriate?
        /// <summary>
        /// Construct ContactJacobianBodyParameters for the pair of potentially contacting bodies and ContactJacobianContactParameters per contact point.
        /// This primes data for faster solving in a solver loop.
        /// </summary>
        /// <param name="perContactParameters">Parameters per contact that should be generated to avoid recomputation in the solver loop.
        /// The elements of the span can be uninitialized memory.</param>
        /// <param name="bodyParameters">Parameters for the pair that should be generated to avoid recomputation in the solver loop</param>
        /// <param name="inertialPoseWorldTransformA">The world-space transform of the center of mass and inertia tensor diagonal orientation for the first body.
        /// Can be identity for a static body.</param>
        /// <param name="velocityA">The velocity of the first body. Can be default for a static body.</param>
        /// <param name="massA">The mass of the first body. Can be default for a static body.</param>
        /// <param name="inertialPoseWorldTransformB">The world-space transform of the center of mass and inertia tensor diagonal orientation for the second body.
        /// Can be identity for a static body.</param>
        /// <param name="velocityB">The velocity of the second body. Can be default for a static body.</param>
        /// <param name="massB">The mass of the second body. Can be default for a static body.</param>
        /// <param name="contactNormal">The contact normal that points from collider B to collider A (when no penetration is present)</param>
        /// <param name="contacts">The list of contact points which lie on the surface of collider B</param>
        /// <param name="coefficientOfRestitution">The bounciness of the contact in the range [0, 1]</param>
        /// <param name="coefficientOfFriction">The amount of "grip" between the surfaces in the range [0, 1]</param>
        /// <param name="maxDepenetrationVelocity">The amount of velocity to be added to depenetrate bodies that are initially overlapping</param>
        /// <param name="gravityAgainstContactNormal">The maximum amount of gravity being applied opposite to the contact normal.
        /// You can simply use the magnitude of the gravity vector for this.</param>
        /// <param name="substep">The time substep duration</param>
        /// <param name="inverseSubstep">The inverse of the substep</param>
        /// <param name="numSubsteps">The number of substeps used in the full physics step</param>
        public static void BuildJacobian(Span<ContactJacobianContactParameters> perContactParameters, out ContactJacobianBodyParameters bodyParameters,
                                         RigidTransform inertialPoseWorldTransformA, in Velocity velocityA, in Mass massA,
                                         RigidTransform inertialPoseWorldTransformB, in Velocity velocityB, in Mass massB,
                                         float3 contactNormal, ReadOnlySpan<ContactsBetweenResult.ContactOnB> contacts,
                                         float coefficientOfRestitution, float coefficientOfFriction,
                                         float maxDepenetrationVelocity, float gravityAgainstContactNormal,
                                         float substep, float inverseSubstep, int numSubsteps = 1)
        {
            CheckContactAndJacobianSpanLengthsEqual(perContactParameters.Length, contacts.Length);

            var    sumInverseMasses = massA.inverseMass + massB.inverseMass;
            var    inverseRotationA = math.conjugate(inertialPoseWorldTransformA.rot);
            var    inverseRotationB = math.conjugate(inertialPoseWorldTransformB.rot);
            float3 centerA          = 0f;
            float3 centerB          = 0f;

            // Indicator whether restitution will be applied,
            // used to scale down friction on bounce.
            bool applyRestitution = false;

            for (int i = 0; i < perContactParameters.Length; i++)
            {
                // Build the jacobian
                ref var jacAngular = ref perContactParameters[i];
                var     contact    = contacts[i];
                float3  pointOnB   = contact.location;
                float3  pointOnA   = contact.location + contactNormal * contact.distanceToA;
                float3  armA       = pointOnA - inertialPoseWorldTransformA.pos;
                float3  armB       = pointOnB - inertialPoseWorldTransformB.pos;
                BuildJacobianAngular(inverseRotationA, inverseRotationB, contactNormal, armA, armB, massA.inverseInertia, massB.inverseInertia, sumInverseMasses,
                                     out jacAngular.jacobianAngular.angularA, out jacAngular.jacobianAngular.angularB, out float invEffectiveMass);
                jacAngular.jacobianAngular.effectiveMass = 1.0f / invEffectiveMass;
                jacAngular.applyImpulse                  = true;

                // Collision detection is performed once per frame. Contact.Distance is estimated from narrowphase and
                // represents the distance between the body and a contact point. solveVelocity represents the velocity
                // the body needs to have to reach contact.Distance in frame timestep. If we take smaller timesteps with
                // substepping, we will travel less distance, but velocity should remain constant
                float solveDistance = contact.distanceToA;  // Distance per frame timestep
                float solveVelocity = solveDistance * inverseSubstep;  // velocity to reach contact plane by end of frame

                // how much velocity needs to be applied to depenetrate the body
                // if solveVelocity = -1 --> VelToReachCp = +1 (is depenetrating), solveDistance < 0, contact.Distance < 0
                // if solveVelocity = +1 --> VelToReachCp = -1   (is approaching), solveDistance > 0
                solveVelocity                          = math.max(-maxDepenetrationVelocity, solveVelocity);
                jacAngular.velocityToReachContactPlane = -solveVelocity;
                jacAngular.contactDistance             = contact.distanceToA;

                // Calculate average position for friction
                centerA += armA;
                centerB += armB;

                // Restitution (optional)
                if (coefficientOfRestitution > 0.0f)
                {
                    var dv = CalculateRelativeVelocityAlongNormal(in velocityA, in velocityB, in jacAngular, contactNormal, out var relativeVelocity);

                    bool applyRestitutionForContact = false;
                    if (numSubsteps > 1)
                    {
                        // Determine if the contact is reached during this substep: if the distance
                        // travelled by the end of this substep results in a penetration, then we need
                        // to bounce now.
                        float distanceTraveled = substep * relativeVelocity;
                        float newDistanceToCp  = jacAngular.contactDistance + distanceTraveled;
                        if (newDistanceToCp < 0.0f)
                        {
                            applyRestitutionForContact = CalculateRestitution(substep,
                                                                              gravityAgainstContactNormal, coefficientOfRestitution,
                                                                              ref jacAngular, relativeVelocity, jacAngular.contactDistance, dv);
                        }

                        // If VelToReachCp was updated in CalculateRestution, update contact distance to 0.
                        jacAngular.contactDistance  = math.select(newDistanceToCp, 0f, applyRestitutionForContact);
                        jacAngular.applyImpulse    &= !applyRestitutionForContact;
                    }
                    else
                    {
                        applyRestitutionForContact = CalculateRestitution(substep,
                                                                          gravityAgainstContactNormal, coefficientOfRestitution,
                                                                          ref jacAngular, relativeVelocity, jacAngular.contactDistance, dv);
                    }
                    applyRestitution |= applyRestitutionForContact;
                }
            }

            bodyParameters                          = default;
            bodyParameters.coefficientOfFriction    = coefficientOfFriction;
            bodyParameters.coefficientOfRestitution = coefficientOfRestitution;
            bodyParameters.contactNormal            = contactNormal;
            bodyParameters.centerA                  = centerA;
            bodyParameters.centerB                  = centerB;
            bodyParameters.substep                  = substep;

            // Build friction jacobians
            BuildFrictionJacobians(ref bodyParameters,
                                   ref centerA,
                                   ref centerB,
                                   contacts.Length,
                                   contactNormal,
                                   inverseRotationA,
                                   inverseRotationB,
                                   massA.inverseInertia,
                                   massB.inverseInertia,
                                   sumInverseMasses);

            // Reduce friction to 1/4 of the impulse if there will be restitution
            if (applyRestitution)
            {
                bodyParameters.friction0.effectiveMass       *= 0.25f;
                bodyParameters.friction1.effectiveMass       *= 0.25f;
                bodyParameters.angularFriction.effectiveMass *= 0.25f;
                bodyParameters.frictionEffectiveMassOffDiag  *= 0.25f;
            }
        }

        /// <summary>
        /// Updates ContactJacobianBodyParameters for the pair of potentially contacting bodies and ContactJacobianContactParameters per contact point.
        /// This is used between each substep for a TGS solver.
        /// </summary>
        /// <param name="perContactParameters">Parameters per contact that should be generated to avoid recomputation in the solver loop.
        /// The elements of the span can be uninitialized memory.</param>
        /// <param name="bodyParameters">Parameters for the pair that should be generated to avoid recomputation in the solver loop</param>
        /// <param name="perContactImpulses">The perContactImpulses to be sent to the Solve() method. This method simply resets these impulses to 0f.</param>
        /// <param name="inertialPoseWorldTransformA">The world-space transform of the center of mass and inertia tensor diagonal orientation for the first body.
        /// Can be identity for a static body.</param>
        /// <param name="velocityA">The velocity of the first body. Can be default for a static body.</param>
        /// <param name="massA">The mass of the first body. Can be default for a static body.</param>
        /// <param name="inertialPoseWorldTransformB">The world-space transform of the center of mass and inertia tensor diagonal orientation for the second body.
        /// Can be identity for a static body.</param>
        /// <param name="velocityB">The velocity of the second body. Can be default for a static body.</param>
        /// <param name="massB">The mass of the second body. Can be default for a static body.</param>
        /// <param name="substep">The time substep duration</param>
        /// <param name="gravityAgainstContactNormal">The maximum amount of gravity being applied opposite to the contact normal.
        /// You can simply use the magnitude of the gravity vector for this.</param>
        public static void UpdateJacobian(Span<ContactJacobianContactParameters> perContactParameters, ref ContactJacobianBodyParameters bodyParameters,
                                          Span<float> perContactImpulses,
                                          RigidTransform inertialPoseWorldTransformA, in Velocity velocityA, in Mass massA,
                                          RigidTransform inertialPoseWorldTransformB, in Velocity velocityB, in Mass massB,
                                          float substep, float gravityAgainstContactNormal)
        {
            perContactImpulses.Clear();

            float sumInvMass       = massA.inverseMass + massB.inverseMass;
            var   inverseRotationA = math.conjugate(inertialPoseWorldTransformA.rot);
            var   inverseRotationB = math.conjugate(inertialPoseWorldTransformB.rot);

            bool applyRestitution = false;  // tracks if a bounce has been solved for the current iteration. Used to update friction
            for (int j = 0; j < perContactParameters.Length; j++)
            {
                ref var jacAngular = ref perContactParameters[j];

                float dv = CalculateRelativeVelocityAlongNormal(velocityA, velocityB, in jacAngular, bodyParameters.contactNormal,
                                                                out float relativeVelocity);

                // Determine if the contact is reached during this substep: if the distance
                // travelled by the end of this substep results in a penetration, then we need
                // to bounce now.
                float distanceTraveled = substep * relativeVelocity;
                float newDistanceToCp  = jacAngular.contactDistance + distanceTraveled;

                var applyRestitutionForContact = false;
                if (bodyParameters.coefficientOfRestitution > 0.0f)
                {
                    if (newDistanceToCp < 0.0f)
                    {
                        applyRestitutionForContact = CalculateRestitution(substep, gravityAgainstContactNormal,
                                                                          bodyParameters.coefficientOfRestitution, ref jacAngular, relativeVelocity, jacAngular.contactDistance,
                                                                          dv);
                    }
                }

                jacAngular.contactDistance  = newDistanceToCp;
                applyRestitution           |= applyRestitutionForContact;
                jacAngular.contactDistance  = math.select(jacAngular.contactDistance, 0f, applyRestitutionForContact);  //0f used as a flag during SolveContact to indicate impulses need to be calculated
                jacAngular.applyImpulse    &= applyRestitutionForContact && newDistanceToCp <= 0f;
            }

            // TODO: if ApplyImpulse= false for all NumContacts, can skip friction building
            {
                // Todo: I think passing centerA and centerB here by ref is a bug in Unity Physics.
                // When building, local variables are passed in after these members are assigned by those locals.
                // This means on the first two substeps, the result is the same, but after each additional substep,
                // the result is modified to get progressively closer to (0, 0, 0) as these values are divided by
                // the contact count.
                BuildFrictionJacobians(ref bodyParameters,
                                       ref bodyParameters.centerA,
                                       ref bodyParameters.centerB,
                                       perContactParameters.Length,
                                       bodyParameters.contactNormal,
                                       inverseRotationA,
                                       inverseRotationB,
                                       massA.inverseInertia,
                                       massB.inverseInertia,
                                       sumInvMass);

                // Reduce friction by 1/4 if there was restitution applied on any contact point
                if (applyRestitution)
                {
                    bodyParameters.friction0.effectiveMass       *= 0.25f;
                    bodyParameters.friction1.effectiveMass       *= 0.25f;
                    bodyParameters.angularFriction.effectiveMass *= 0.25f;
                    bodyParameters.frictionEffectiveMassOffDiag  *= 0.25f;
                }
            }

            bodyParameters.substep = substep;
        }

        // Returns true if a collision event was detected.
        // perContactImpulses should be initialized to 0f prior to the first solver iteration call (unless you know what you are doing).
        /// <summary>
        /// Performs a solver iteration between the two potentially contacting bodies, updating velocity values and computing impulses.
        /// </summary>
        /// <param name="velocityA">The velocity of the first body. For a static body, this can be stack-allocated to a default value and discarded afterwards.</param>
        /// <param name="massA">The mass of the first body. Can be default for a static body.</param>
        /// <param name="motionStabilizationSolverInputA">Stabilization state for the first body. Can be set to kDefault when not using stabilization.</param>
        /// <param name="velocityB">The velocity of the second body. For a static body, this can be stack-allocated to a default value and discarded afterwards.</param>
        /// <param name="massB">The mass of the second body. Can be default for a static body.</param>
        /// <param name="motionStabilizationSolverInputB">Stabilization state for the first body. Can be set to kDefault when not using stabilization.</param>
        /// <param name="perContactParameters">Parameters per contact that describe how each speculative contact point may approach actual contact relative to velocity</param>
        /// <param name="perContactImpulses">Accummulated impulses per contact from each solver iteration. These should be initialized to 0f prior to the first solver iteration.</param>
        /// <param name="bodyParameters">Parameters between the bodies that describes friction characteristics and the contact plane.</param>
        /// <param name="enableFrictionVelocitiesHeuristic">If true, enables the friction velocities heuristic from the motion stabilization inputs.</param>
        /// <param name="invNumSolverIterations">The reciprocal of the number of solver iterations. That is, for 4 solver iterations, this would be 0.25f</param>
        /// <param name="outputImpulses">Impulses computed by this solver iteration</param>
        /// <param name="collisionFilter">An optional collision filter used to filter out contacts that are likely to produce ghost collisions</param>
        /// <returns>True if a collision was detected during this solve operation, meaning that a collision occurred within the timestep</returns>
        public static bool SolveJacobian(ref Velocity velocityA, in Mass massA, in MotionStabilizer motionStabilizationSolverInputA,
                                         ref Velocity velocityB, in Mass massB, in MotionStabilizer motionStabilizationSolverInputB,
                                         ReadOnlySpan<ContactJacobianContactParameters> perContactParameters, Span<float> perContactImpulses,
                                         in ContactJacobianBodyParameters bodyParameters,
                                         bool enableFrictionVelocitiesHeuristic, float invNumSolverIterations,
                                         out ContactJacobianImpulses outputImpulses, in ContactJacobianStaticCollisionFilter collisionFilter = default)
        {
            CheckContactAndImpulseSpanLengthsEqual(perContactParameters.Length, perContactImpulses.Length);

            // Copy velocity data
            Velocity tempVelocityA = velocityA;
            Velocity tempVelocityB = velocityB;

            // Solve normal impulses
            bool  hasCollisionEvent = false;
            float sumImpulses       = 0.0f;
            outputImpulses          = default;

            var validContacts = WillContactsHit(in collisionFilter, in perContactParameters, velocityA, velocityB, bodyParameters.substep);

            for (int j = validContacts.CountTrailingZeros(); j < 32; validContacts.SetBits(j, false), j = validContacts.CountTrailingZeros())
            {
                ref readonly ContactJacobianContactParameters jacAngular     = ref perContactParameters[j];
                var                                           contactImpulse = perContactImpulses[j];

                // Solve velocity so that predicted contact distance is greater than or equal to zero
                // applyImpulse is required check for substepping because having a contact is not guaranteed. We need to check if the contact point has been reached this step
                // (when ContactDistance <= 0), otherwise, we need to ensure that no impulse will be applied.
                var dv = jacAngular.applyImpulse ? CalculateRelativeVelocityAlongNormal(in tempVelocityA, in tempVelocityB, in jacAngular, bodyParameters.contactNormal, out _) : 0f;
                //var dv = CalculateRelativeVelocityAlongNormal(in velocityA, in velocityB, in jacAngular, bodyParameters.contactNormal, out var relVel);

                float impulse = dv * jacAngular.jacobianAngular.effectiveMass;

                // Accumulation of impulses for each contact point over numSolverIterations. Cannot be negative
                float accumulatedImpulse = math.max(contactImpulse + impulse, 0.0f);
                float deltaImpulse       = 0f;
                if (accumulatedImpulse != contactImpulse)
                {
                    deltaImpulse = accumulatedImpulse - contactImpulse;
                    ApplyImpulse(deltaImpulse, bodyParameters.contactNormal, jacAngular.jacobianAngular, ref tempVelocityA, ref tempVelocityB, in massA, in massB,
                                 motionStabilizationSolverInputA.inverseInertiaScale, motionStabilizationSolverInputB.inverseInertiaScale);
                    //UnityEngine.Debug.Log($"contact: {j} impulse: {deltaImpulse}, new velocity: {tempVelocityA.angular}, old velocity: {velocityA.angular}");
                    //if (math.abs(tempVelocityA.linear.z) > 0.25f)
                    //{
                    //    UnityEngine.Debug.Log(
                    //        $"Contact is culprit. z = {tempVelocityA.linear.z}, before: {velocityA.linear.z}, contactNormal: {bodyParameters.contactNormal}, relVel: {relVel}, velToContactPlane: {jacAngular.velocityToReachContactPlane}");
                    //}
                }

                contactImpulse                               = accumulatedImpulse;
                perContactImpulses[j]                        = contactImpulse;
                sumImpulses                                 += accumulatedImpulse;
                outputImpulses.combinedContactPointsImpulse += deltaImpulse;

                // Force contact event even when no impulse is applied, but there is penetration.
                hasCollisionEvent |= jacAngular.velocityToReachContactPlane > 0.0f;
            }

            // Export collision event
            hasCollisionEvent |= outputImpulses.combinedContactPointsImpulse > 0.0f;

            // Todo: sumImpulses is used to model friction. However, if each contact ends up on a different triangle,
            // friction is applied before values can cancel out, which creates a biased slide effect.

            // Solve friction
            if (sumImpulses > 0.0f)
            {
                // Choose friction axes
                mathex.GetDualPerpendicularNormalized(bodyParameters.contactNormal, out float3 frictionDir0, out float3 frictionDir1);

                // Calculate impulses for full stop
                float3 imp;
                {
                    // Take velocities that produce minimum energy (between input and solver velocity) as friction input
                    float3 frictionLinVelA = tempVelocityA.linear;
                    float3 frictionAngVelA = tempVelocityA.angular;
                    float3 frictionLinVelB = tempVelocityB.linear;
                    float3 frictionAngVelB = tempVelocityB.angular;
                    if (enableFrictionVelocitiesHeuristic)
                    {
                        // if tempVelocityA.HasInfiniteMass || tempVelocityB.HasInfiniteMass, then GetFrictionVelocities calcs NaN and Inf with no warnings
                        GetFrictionVelocities(motionStabilizationSolverInputA.inputVelocity.linear, motionStabilizationSolverInputA.inputVelocity.angular,
                                              tempVelocityA.linear, tempVelocityA.angular,
                                              math.rcp(massA.inverseInertia), math.rcp(massA.inverseMass),
                                              out frictionLinVelA, out frictionAngVelA);
                        GetFrictionVelocities(motionStabilizationSolverInputB.inputVelocity.linear, motionStabilizationSolverInputB.inputVelocity.angular,
                                              tempVelocityB.linear, tempVelocityB.angular,
                                              math.rcp(massB.inverseInertia), math.rcp(massB.inverseMass),
                                              out frictionLinVelB, out frictionAngVelB);
                    }

                    // Calculate the jacobian dot velocity for each of the friction jacobians
                    float dv0 = bodyParameters.surfaceVelocityDv.x - GetJacVelocity(frictionDir0,
                                                                                    bodyParameters.friction0,
                                                                                    frictionLinVelA,
                                                                                    frictionAngVelA,
                                                                                    frictionLinVelB,
                                                                                    frictionAngVelB);
                    float dv1 = bodyParameters.surfaceVelocityDv.y - GetJacVelocity(frictionDir1,
                                                                                    bodyParameters.friction1,
                                                                                    frictionLinVelA,
                                                                                    frictionAngVelA,
                                                                                    frictionLinVelB,
                                                                                    frictionAngVelB);
                    float dva = bodyParameters.surfaceVelocityDv.z - math.csum(
                        bodyParameters.angularFriction.angularA * frictionAngVelA + bodyParameters.angularFriction.angularB * frictionAngVelB);

                    // Reassemble the effective mass matrix
                    float3 effectiveMassDiag = new float3(bodyParameters.friction0.effectiveMass,
                                                          bodyParameters.friction1.effectiveMass,
                                                          bodyParameters.angularFriction.effectiveMass);
                    float3x3 effectiveMass = BuildSymmetricMatrix(effectiveMassDiag, bodyParameters.frictionEffectiveMassOffDiag);

                    // Calculate the impulse
                    imp = math.mul(effectiveMass, new float3(dv0, dv1, dva));
                    //UnityEngine.Debug.Log(
                    //    $"effectiveMass: {effectiveMass}, dv01a: {new float3(dv0, dv1, dva)}, surfaceVel: {bodyParameters.surfaceVelocityDv}, friction1: {bodyParameters.friction1.angularA}, {bodyParameters.friction1.angularB}, linVels: {frictionLinVelA}, {frictionAngVelA}, {frictionLinVelB}, {frictionAngVelB}");
                }

                // Clip TODO.ma calculate some contact radius and use it to influence balance between linear and angular friction
                float maxImpulse              = sumImpulses * bodyParameters.coefficientOfFriction * invNumSolverIterations;
                float frictionImpulseSquared  = math.lengthsq(imp);
                imp                          *= math.min(1.0f, maxImpulse * math.rsqrt(frictionImpulseSquared));

                // Apply impulses
                var beforeFriction = tempVelocityA.linear.z;
                ApplyImpulse(imp.x, frictionDir0, bodyParameters.friction0, ref tempVelocityA, ref tempVelocityB,
                             in massA, in massB,
                             motionStabilizationSolverInputA.inverseInertiaScale, motionStabilizationSolverInputB.inverseInertiaScale);
                ApplyImpulse(imp.y, frictionDir1, bodyParameters.friction1, ref tempVelocityA, ref tempVelocityB,
                             in massA, in massB,
                             motionStabilizationSolverInputA.inverseInertiaScale, motionStabilizationSolverInputB.inverseInertiaScale);
                //if (math.abs(tempVelocityA.linear.z) > 0.25f)
                //    UnityEngine.Debug.Log(
                //        $"Friction is culprit. z = {tempVelocityA.linear.z}, before: {beforeFriction}, beginning: {velocityA.linear.z}, imp: {imp}, frictionDir0: {frictionDir0}, frictionDir1: {frictionDir1}, maxImpulse: {maxImpulse}, frictionImpulseSq: {frictionImpulseSquared}");

                tempVelocityA.angular += imp.z * bodyParameters.angularFriction.angularA * motionStabilizationSolverInputA.inverseInertiaScale * massA.inverseInertia;
                tempVelocityB.angular += imp.z * bodyParameters.angularFriction.angularB * motionStabilizationSolverInputB.inverseInertiaScale * massB.inverseInertia;

                // Accumulate them
                outputImpulses.friction0Impulse       = imp.x;
                outputImpulses.friction1Impulse       = imp.y;
                outputImpulses.frictionAngularImpulse = imp.z;
            }

            // Write back linear and angular velocities. Changes to other properties, like InverseMass, should not be persisted.
            velocityA = tempVelocityA;
            velocityB = tempVelocityB;

            return hasCollisionEvent;
        }

        #region Internal Helpers
        private static void ApplyImpulse(
            float impulse, float3 linear, ContactJacobianAngular jacAngular,
            ref Velocity velocityA, ref Velocity velocityB,
            in Mass massA, in Mass massB,
            float inverseInertiaScaleA = 1.0f, float inverseInertiaScaleB = 1.0f)
        {
            velocityA.linear += impulse * linear * massA.inverseMass;
            velocityB.linear -= impulse * linear * massB.inverseMass;

            // Scale the impulse with inverseInertiaScale
            velocityA.angular += impulse * jacAngular.angularA * inverseInertiaScaleA * massA.inverseInertia;
            velocityB.angular += impulse * jacAngular.angularB * inverseInertiaScaleB * massB.inverseInertia;
        }

        static void BuildFrictionJacobians(ref ContactJacobianBodyParameters bodyParameters, ref float3 centerA, ref float3 centerB,
                                           int numContacts, float3 contactNormal,
                                           quaternion inverseRotationA, quaternion inverseRotationB,
                                           float3 inverseInertiaA, float3 inverseInertiaB, float sumInverseMasses)
        {
            // Calculate average position
            float invNumContacts  = math.rcp(numContacts);
            centerA              *= invNumContacts;
            centerB              *= invNumContacts;

            // Choose friction axes
            mathex.GetDualPerpendicularNormalized(contactNormal, out float3 frictionDir0, out float3 frictionDir1);
            bodyParameters.frictionDirection0 = frictionDir0;
            bodyParameters.frictionDirection1 = frictionDir1;

            // Build linear jacobian
            float invEffectiveMass0, invEffectiveMass1;
            {
                float3 armA = centerA;
                float3 armB = centerB;
                BuildJacobianAngular(inverseRotationA, inverseRotationB, frictionDir0, armA, armB, inverseInertiaA, inverseInertiaB, sumInverseMasses,
                                     out bodyParameters.friction0.angularA, out bodyParameters.friction0.angularB, out invEffectiveMass0);
                BuildJacobianAngular(inverseRotationA, inverseRotationB, frictionDir1, armA, armB, inverseInertiaA, inverseInertiaB, sumInverseMasses,
                                     out bodyParameters.friction1.angularA, out bodyParameters.friction1.angularB, out invEffectiveMass1);
            }

            // Build angular jacobian
            float invEffectiveMassAngular;
            {
                bodyParameters.angularFriction.angularA  = math.mul(inverseRotationA, contactNormal);
                bodyParameters.angularFriction.angularB  = math.mul(inverseRotationB, -contactNormal);
                float3 temp                              = bodyParameters.angularFriction.angularA * bodyParameters.angularFriction.angularA * inverseInertiaA;
                temp                                    += bodyParameters.angularFriction.angularB * bodyParameters.angularFriction.angularB * inverseInertiaB;
                invEffectiveMassAngular                  = math.csum(temp);
            }

            // Build effective mass
            {
                // Build the inverse effective mass matrix
                var invEffectiveMassDiag    = new float3(invEffectiveMass0, invEffectiveMass1, invEffectiveMassAngular);
                var invEffectiveMassOffDiag = new float3(  // (0, 1), (0, 2), (1, 2)
                    CalculateInvEffectiveMassOffDiag(bodyParameters.friction0.angularA, bodyParameters.friction1.angularA,       inverseInertiaA,
                                                     bodyParameters.friction0.angularB, bodyParameters.friction1.angularB, inverseInertiaB),
                    CalculateInvEffectiveMassOffDiag(bodyParameters.friction0.angularA, bodyParameters.angularFriction.angularA, inverseInertiaA,
                                                     bodyParameters.friction0.angularB, bodyParameters.angularFriction.angularB, inverseInertiaB),
                    CalculateInvEffectiveMassOffDiag(bodyParameters.friction1.angularA, bodyParameters.angularFriction.angularA, inverseInertiaA,
                                                     bodyParameters.friction1.angularB, bodyParameters.angularFriction.angularB, inverseInertiaB));

                // Invert the matrix and store it to the jacobians
                if (!InvertSymmetricMatrix(invEffectiveMassDiag, invEffectiveMassOffDiag, out float3 effectiveMassDiag, out float3 effectiveMassOffDiag))
                {
                    // invEffectiveMass can be singular if the bodies have infinite inertia about the normal.
                    // In that case angular friction does nothing so we can regularize the matrix, set col2 = row2 = (0, 0, 1)
                    invEffectiveMassOffDiag.y = 0.0f;
                    invEffectiveMassOffDiag.z = 0.0f;
                    invEffectiveMassDiag.z    = 1.0f;
                    bool success              = InvertSymmetricMatrix(invEffectiveMassDiag,
                                                                      invEffectiveMassOffDiag,
                                                                      out effectiveMassDiag,
                                                                      out effectiveMassOffDiag);
                    Unity.Assertions.Assert.IsTrue(success);  // it should never fail, if it does then friction will be disabled
                }
                bodyParameters.friction0.effectiveMass       = effectiveMassDiag.x;
                bodyParameters.friction1.effectiveMass       = effectiveMassDiag.y;
                bodyParameters.angularFriction.effectiveMass = effectiveMassDiag.z;
                bodyParameters.frictionEffectiveMassOffDiag  = effectiveMassOffDiag;
            }
        }

        static void BuildJacobianAngular(quaternion inverseRotationA, quaternion inverseRotationB, float3 normal, float3 armA, float3 armB,
                                         float3 invInertiaA, float3 invInertiaB, float sumInvMass, out float3 angularA, out float3 angularB, out float invEffectiveMass)
        {
            float3 crossA = math.cross(armA, normal);
            angularA      = math.mul(inverseRotationA, crossA).xyz;

            float3 crossB = math.cross(normal, armB);
            angularB      = math.mul(inverseRotationB, crossB).xyz;

            float3 temp      = angularA * angularA * invInertiaA + angularB * angularB * invInertiaB;
            invEffectiveMass = temp.x + temp.y + temp.z + sumInvMass;
        }

        // Builds a symmetric 3x3 matrix from diag = (0, 0), (1, 1), (2, 2), offDiag = (0, 1), (0, 2), (1, 2) = (1, 0), (2, 0), (2, 1)
        static float3x3 BuildSymmetricMatrix(float3 diag, float3 offDiag)
        {
            return new float3x3(
                new float3(diag.x, offDiag.x, offDiag.y),
                new float3(offDiag.x, diag.y, offDiag.z),
                new float3(offDiag.y, offDiag.z, diag.z)
                );
        }

        // Calculates the relative velocity between two bodies in the normal direction.
        // Reminder:
        // (is depenetrating) velocityToReachContactPlane > 0, solveVelocity < 0, solveDistance < 0, contact.Distance < 0.
        // (is approaching)   velocityToReachContactPlane < 0, solveVelocity > 0, solveDistance > 0, contact.Distance > 0.
        // Therefore, if velocityToReachContactPlane - relativeVelocity > 0, then velocityToReachContactPlane > relativeVelocity
        // if velocityToReachContactPlane > 0 and velocityToReachContactPlane > relativeVelocity,
        //      there is depenetration and the bodies can't move fast enough to resolve the penetration
        // if velocityToReachContactPlane < 0 and velocityToReachContactPlane < relativeVelocity, then there is nothing to worry about
        static float CalculateRelativeVelocityAlongNormal(in Velocity velocityA,
                                                          in Velocity velocityB,
                                                          in ContactJacobianContactParameters contact,
                                                          float3 normal,
                                                          out float relativeVelocity)
        {
            relativeVelocity = GetJacVelocity(normal, in contact.jacobianAngular, velocityA.linear, velocityA.angular, velocityB.linear, velocityB.angular);
            return contact.velocityToReachContactPlane - relativeVelocity;
        }

        /// <summary>
        /// This method calculates the VelToReachCp correction so that the Solve will calculate an accurate impulse to
        /// redirect motion to apply a restitution effect. This is calculated using two fundamental physics equations:
        /// 1) v1 = v2 - at         [Remove gravity integration]
        ///     where v2 = relativeVelocity, at = -length(gravity) * timestep, and
        ///     v1 is the velocity without gravity applied (velocityBeforeBounce). This is scaled by the coefficient of restitution.
        ///     We are assuming that this is the maximum velocity of the body and that it is now at the contact point.
        ///     Gravity integration is removed because it can't fall anymore.
        /// 2) v2^2 = v1^2 + 2ad    [Calculate restitution velocity as body moves against gravity]
        ///     where a = acceleration due to gravity (and is negative), and d = delta distance (d2-d1).
        ///     This equation dictates that v2>v1 if "d<0", which is the case when a body is falling and d2=0. We are
        ///     however going to let d1=currentDistanceToCp and d2=0, which means that v1 = velocityBeforeBounce (v1 in eq'n1)
        ///     and v2 = velocityRebound (v1>v2). This rebound velocity is the velocity of the body after the restitution
        ///     and is used to correct the VelToReachCp.
        ///
        /// This method updates jacAngular.VelToReachCp and this updated value will always be positive.
        ///
        /// This is an approximation for 2 reasons:
        /// - The restitution impulse is applied as if contact point is on the contact plane. We are assuming the contact
        /// point will hit the contact plane with its current velocity, while actually it would have a portion of gravity
        /// applied before the actual hit. However, that velocity increase is less than gravity in one step.
        /// - gravityMagnitude is the actual value of gravity applied only when contact plane is directly opposite to
        /// gravity direction. Otherwise, this value will only be smaller. However, since this can only result in
        /// smaller bounce than the "correct" one, we can safely go with the default gravity value in all cases.
        /// </summary>
        /// <param name="timestep">  The substep timestep. </param>
        /// <param name="gravityMagnitude">  A float value of the length of the gravity vector.</param>
        /// <param name="coefficientOfRestitution">  The coefficient of Restitution. </param>
        /// <param name="jacAngular"></param>
        /// <param name="relativeVelocity">  The relative velocity between the two bodies at the current point in time. </param>
        /// <param name="currentDistanceToCp">  The contact point distance. This is equivalent to -jacAngular.VelToReachCp * timestep  </param>
        /// <param name="dv">  Delta Velocity: The difference between the velocity required to reach the contact point and the actual velocity </param>
        /// <returns></returns>
        static bool CalculateRestitution(float timestep, float gravityMagnitude, float coefficientOfRestitution,
                                         ref ContactJacobianContactParameters jacAngular, float relativeVelocity, float currentDistanceToCp, float dv)
        {
            bool applyRestitution = false;

            //TODO: need to account for velocity.GravityFactor (A? B? Both? How combine?)
            float negContactRestingVelocity = -gravityMagnitude * timestep;

            // Conditions to apply restitution:
            // 1) dv: The relative velocity is larger than the velocity threshold to reach the contact point
            // 2) Gravity threshold: The relative velocity must be larger than the 'at rest/gravity only' velocity
            if (dv > 0.0f && relativeVelocity < negContactRestingVelocity)
            {
                // Use equation 1 to calculate v1 (velocityBeforeBounce)
                float velocityBeforeBounce = (relativeVelocity - negContactRestingVelocity) * coefficientOfRestitution;
                float distanceToGround     = math.max(currentDistanceToCp, 0.0f);

                // Use equation 2 to calculate rebound velocity (v1) based on v2 (the velocity at the bounce)
                float velocityRebound = math.sqrt(math.max(velocityBeforeBounce * velocityBeforeBounce - 2.0f * -gravityMagnitude * -distanceToGround, 0.0f));

                jacAngular.velocityToReachContactPlane = math.max(jacAngular.velocityToReachContactPlane - velocityRebound, 0.0f) + velocityRebound;
                applyRestitution                       = true;
            }

            return applyRestitution;
        }

        static void GetFrictionVelocities(
            float3 inputLinearVelocity, float3 inputAngularVelocity,
            float3 intermediateLinearVelocity, float3 intermediateAngularVelocity,
            float3 inertia, float mass,
            out float3 frictionLinearVelocityOut, out float3 frictionAngularVelocityOut)
        {
            float inputEnergy;
            {
                float linearEnergySq  = mass * math.lengthsq(inputLinearVelocity);
                float angularEnergySq = math.dot(inertia * inputAngularVelocity, inputAngularVelocity);
                inputEnergy           = linearEnergySq + angularEnergySq;
            }

            float intermediateEnergy;
            {
                float linearEnergySq  = mass * math.lengthsq(intermediateLinearVelocity);
                float angularEnergySq = math.dot(inertia * intermediateAngularVelocity, intermediateAngularVelocity);
                intermediateEnergy    = linearEnergySq + angularEnergySq;
            }

            if (inputEnergy < intermediateEnergy)
            {
                // Make sure we don't change the sign of intermediate velocity when using the input one.
                // If sign was to be changed, zero it out since it produces less energy.
                bool3 changedSignLin       = inputLinearVelocity * intermediateLinearVelocity < float3.zero;
                bool3 changedSignAng       = inputAngularVelocity * intermediateAngularVelocity < float3.zero;
                frictionLinearVelocityOut  = math.select(inputLinearVelocity, float3.zero, changedSignLin);
                frictionAngularVelocityOut = math.select(inputAngularVelocity, float3.zero, changedSignAng);
            }
            else
            {
                frictionLinearVelocityOut  = intermediateLinearVelocity;
                frictionAngularVelocityOut = intermediateAngularVelocity;
            }
        }

        // Returns the relative velocity between the dispatch pairs involved in the contact that are aligned with the contact normal
        // linear = contact normal
        static float GetJacVelocity(float3 linear, in ContactJacobianAngular jacAngular,
                                    float3 linVelA, float3 angVelA, float3 linVelB, float3 angVelB)
        {
            float3 temp  = (linVelA - linVelB) * linear;
            temp        += angVelA * jacAngular.angularA;
            temp        += angVelB * jacAngular.angularB;
            return math.csum(temp);
        }

        static BitField64 WillContactsHit(in ContactJacobianStaticCollisionFilter collisionFilter,
                                          in ReadOnlySpan<ContactJacobianContactParameters> parameters,
                                          Velocity velocityA,
                                          Velocity velocityB,
                                          float deltaTime)
        {
            BitField64 result = default;
            result.SetBits(0, true, parameters.Length);
            if (!collisionFilter.enabled)
                return result;

            // Unity's algorithm for this doesn't make a whole lot of sense. I believe the idea is that each contact point
            // raycasts against the other static collider, and if it misses, then the contact should be ignored.
            // There are many problems with Unity's implementation:
            // 1) Contacts are usually chosen on the edges of geometry, so slight movements will result in incorrect behavior
            // on cliff edges.
            // 2) Unity Physics only allocates a single contact point in the stream for this, and every contact evaluated at
            // build time writes to this single value. This effectively means the last contact in the contacts array is the
            // one tested, except...
            // 3) Unity Physics uses the single contact on B, but uses individual contact distances when it needs contacts on A,
            // making the behavior wildly different if A or B is static.
            // 4) The torsion calculation treats the angular velocity as-if it were in world-space.
            // 5) The torsion calculation is a tangential velocity, which is a poor approximation of where the contact will be.
            // 6) The contact does not update with TGS, so it always casts a ray from the beginning of the time step to the end
            // of the first substep.
            var velocity = collisionFilter.aIsStatic ? velocityB : velocityA;
            for (int i = 0; i < parameters.Length; i++)
            {
                // Check if contact is already penetrating
                if (parameters[i].contactDistance <= 0f)
                    continue;

                var contactPoint = collisionFilter.contactPointOnB + collisionFilter.contactDistanceDirectionFactor * parameters[i].contactDistance;

                // Create the predicted contact point which the contact will occur in the next timestep.
                var contactDirection             = contactPoint - collisionFilter.centerOfMass;
                var velocityTorsion              = math.cross(velocity.angular, contactDirection);
                var linearVelocityAtContactPoint = velocityTorsion + velocity.linear;
                var predictedContactPoint        = contactPoint + deltaTime * linearVelocityAtContactPoint;

                if (!Physics.Raycast(new Ray(contactPoint, predictedContactPoint), in collisionFilter.collider, in collisionFilter.transform, collisionFilter.subCollider, out _))
                {
                    result.SetBits(i, false);
                }
            }
            return result;
        }
        #endregion

        [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")]
        static void CheckContactAndJacobianSpanLengthsEqual(int parametersLength, int contactsLength)
        {
            if (parametersLength != contactsLength)
                throw new ArgumentException($"Span<ContactJacobianContactParameters> length of {parametersLength} does not match the number of contacts {contactsLength}");
        }

        [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")]
        static void CheckContactAndImpulseSpanLengthsEqual(int parametersLength, int impulseLength)
        {
            if (impulseLength != parametersLength)
                throw new ArgumentException($"Span<ContactJacobianContactParameters> length of {parametersLength} does not match impulses Span<float> length of {impulseLength}");
        }
    }
}

